#include <core/init.h>
#include <core/irq.h>
#include <core/io.h>
#include <autoconf.h>

static __VOLATILE__ ULONG loop_per_usec;

void udelay(ULONG n)
{
	__VOLATILE__ ULONG m = n * loop_per_usec;


	while (m-- > 0);
}


//fixme: temporary
static ULONG CalcLoopVal(ULONG InsNum)
{
	__VOLATILE__ ULONG CurVal;
	__VOLATILE__ int index = 0;


	CurVal = GuGetTick();

	while (index++ <= InsNum);

	return GuGetTick() - CurVal;
}

static void SetLoopVal(ULONG InsNum, ULONG val)
{
	if (val)
		loop_per_usec = InsNum / val / 1000;

	if (0 == loop_per_usec)
	{
		loop_per_usec = 1;
		printf("Using default value: loop_per_usec = %d\n",
			loop_per_usec);
	}
}

#ifndef TEST_INS_NUM
#define TEST_INS_NUM 100000
#endif

static int At91PitIsr(void *pDev)
{
	if (!(readl(VA(AT91SAM926X_PA_PITC + PITC_SR)) & 0x1))
		return IRQ_NONE;

	GuIncTick();

	readl(VA(AT91SAM926X_PA_PITC + PITC_PIVR));

	return IRQ_HANDLED;
}

static __INIT__ int At91PitInit(void)
{
	int ret;


	SUBSYS_INFO("Timer init\n");

	writel((MASTER_CLOCK_VALUE / 16 / 1000), VA(AT91SAM926X_PA_PITC + PITC_MR));

	ret = RegisterIsr(1, At91PitIsr, NULL);
	if (ret < 0)
	{
		printf("%s(): RegisterIsr() fail!\n", __FUNCTION__);
		return ret;
	}

	writel((0x3 << 24) | readl(VA(AT91SAM926X_PA_PITC + PITC_MR)), VA(AT91SAM926X_PA_PITC + PITC_MR));

	SetLoopVal(TEST_INS_NUM, CalcLoopVal(TEST_INS_NUM));

	return ret;
}

SUBSYS_INIT(At91PitInit);

